#include <common/geometry_calcs.h>
#include <common/parameters.h>
#include "common/types.h"
#include "common/log/QsLog.h"

#include <iostream>
#include <math.h>

using std::cout;
using std::endl;

/**
  *
  */
double deg2rad(double angle)
{
  return (angle * M_PI ) / 180;
}

/**
  *
  */
double get_ta(const Point3D &p0, const Point3D &p1, double radio)
{
    double a, b, c;
    double d, r1, r2;

    double y0 = p0.y;
    double y1 = p1.y;
    double z0 = p0.z;
    double z1 = p1.z;

    a = ((y1 - y0) * (y1 - y0)) + ((z1 - z0) * (z1 - z0));
    b = (2 * y0 * (y1 - y0)) + (2 * z0 * (z1 - z0));
    c = (y0 * y0) + (z0 * z0) - (radio * radio);

    d = (b * b) - (4 * a * c);

    r1 = (-b + sqrt(d)) / (2 * a);
    r2 = (-b - sqrt(d)) / (2 * a);

    if(r1 > 0)
        return r1;
    else if (r2 > 0)
        return r2;
    else
        return 0;
}

/**
  *
  */
Point3D *find_a_point(const Point3D &p0, const Point3D &vector, double lambda)
{
    double x, y, z;

    x = p0.x + lambda * (vector.x);
    y = p0.y + lambda * (vector.y);
    z = p0.z + lambda * (vector.z);

    return new Point3D(x, y, z);
}

/**
  *
  */
Point *lineal_interpolation(const Point &p0, const Point &p1, double x)
{
//    std::cout << "TODO: lineal interpolation" << std::endl;
    Point *point;
    double a, b, c;

    a = (x - p0.x) / (p1.x - p0.x);
    b = p1.y - p0.y;
    c = a * b + p0.y;

    point = new Point(x, c);

    return point;
}

/**
  *
  */
Point3D* get_translation_point(Axis to, double wingspan, double dihedral_angle, double arrow_angle)
{
//    cout << "TODO: get_translation_point" << endl;
    double x, y, z;

    double rdangle = deg2rad(dihedral_angle);
    double raagle = deg2rad(arrow_angle);

    switch(to)
    {
        case AXIS_X:
            x = wingspan;
            y = (wingspan / cos(rdangle)) * sin(rdangle);;
            z = (wingspan / cos(raagle)) * sin(raagle);;
            break;
        case AXIS_Y:
            x = (wingspan / cos(rdangle)) * sin(rdangle);
            y = wingspan;
            z = (wingspan / cos(raagle)) * sin(raagle);
            break;
        case AXIS_Z:
            x = (wingspan / cos(rdangle)) * sin(rdangle);;
            y = (wingspan / cos(raagle)) * sin(raagle);;
            z = wingspan;
            break;
    }

    Point3D *translation_point = new Point3D(x, y, z);

    return translation_point;
}


/**
  *
  */
Point* circunsference_point(Point origin, double radio, double angle)
{
    double rangle = deg2rad(angle);
    double xc = origin.x + radio * cos(rangle);
    double yc = origin.y + radio * sin(rangle);

    return new Point(xc, yc);
}

/**
  *
  */
Point3D* get_point(double radio, double polar_angle, double azimut_angle)
{
    double polar_r = deg2rad(polar_angle);
    double azimut_r = deg2rad(azimut_angle);

    double x = radio * sin(polar_r) * cos(azimut_r);
    double y = radio * sin(polar_r) * sin(azimut_r);
    double z = radio * cos(polar_r);

    return new Point3D(x, y, z);
}

/**
  * Create a semicircle with 'nknots' points, fst included, and stored them in 'output' vector
  */
void create_semicircle(int nknots, Point fst, double radio, QVector<Point *> &output)
{
    int top_knots = nknots / 2  + (nknots % 2);
    int bottom_knots = (nknots / 2);

    double fstp_angle = atan(fst.y / fst.x) * 180 / M_PI;

    double top_angle = 90.0 - fstp_angle;
    double bottom_angle = 90.0 + fstp_angle;

    if(output.size() == 0)
        output.resize(nknots);

    double angle_inc = top_angle / (top_knots - 1);
    double angle = 90.0;
    output[0] = new Point(0.0, radio);
    for(int i = 1; i < top_knots - 1; i++)
    {
        angle -= angle_inc;
        output[i] = circunsference_point(Point(), radio, angle);
    }
    output[top_knots - 1] = new Point(fst.x, fst.y);

    angle_inc = bottom_angle / bottom_knots;
    angle = fstp_angle;
    for(int i = top_knots; i < nknots - 1; i++)
    {
        angle -= angle_inc;
        output[i] = circunsference_point(Point(), radio, angle);
    }
    output[nknots - 1] = new Point(0.0, -radio);
}

/**
  *
  */
Point3D *get_point_using_lineal_interpolation(QVector<Point *> &points, double value)
{
    int i = 1;
    Point *l_interp;
    int asize = points.size();

    while(points.at(asize - i)->x < value && i < points.size())
        i++;

    Point3D *result;
    int c_pos = asize - i;

    if(points.at(c_pos)->x == value)
        result = new Point3D(points.at(c_pos)->x, 0.0, points.at(c_pos)->y);
    else
    {
        l_interp = lineal_interpolation(*points.at(c_pos - 1), *points.at(c_pos), value);
        result = new Point3D(l_interp->x, 0.0, l_interp->y);

        delete l_interp;
    }

    return result;
}

/**
  *
  */
int calculate_nknots(const Point3D &icp1, const Point3D &icp2, const Point3D &fcp)
{
//    cout << "Calculate Knots" << endl;

    double dist1 = icp1.distance(fcp);
    double dist2 = icp1.distance(icp2);

    return dist1 / dist2;
}
